/*
 * w25q.c
 *
 *  Created on: 2021-03-17
 *      Author: chens
 */

#include "w25q.h"
#include "quadspi.h"
#include "Dev_Inf.h"

#define QSPI_INSTRUCTION_0_LINES QSPI_INSTRUCTION_NONE
#define QSPI_INSTRUCTION_1_LINES QSPI_INSTRUCTION_1_LINE
#define QSPI_ADDRESS_0_LINES     QSPI_ADDRESS_NONE
#define QSPI_ADDRESS_1_LINES     QSPI_ADDRESS_1_LINE
#define QSPI_DATA_0_LINES        QSPI_DATA_NONE
#define QSPI_DATA_1_LINES        QSPI_DATA_1_LINE
#define QSPI_ADDRESS_0_BITS      QSPI_ADDRESS_8_BITS

#define __CMD(cmd, instrLine, addrLine, dataLine, addrBits, dummyCycles) cmd
#define __INSTR_MODE(cmd, instrLine, addrLine, dataLine, addrBits, dummyCycles)   QSPI_INSTRUCTION_##instrLine##_LINES
#define __ADDR_MODE(cmd, instrLine, addrLine, dataLine, addrBits, dummyCycles)    QSPI_ADDRESS_##addrLine##_LINES
#define __DATA_MODE(cmd, instrLine, addrLine, dataLine, addrBits, dummyCycles)    QSPI_DATA_##dataLine##_LINES
#define __ADDR_BITS(cmd, instrLine, addrLine, dataLine, addrBits, dummyCycles)    QSPI_ADDRESS_##addrBits##_BITS
#define __DUMMY_CYCLES(cmd, instrLine, addrLine, dataLine, addrBits, dummyCycles) dummyCycles

#define  _CMD(instr)          __CMD instr
#define  _INSTR_MODE(instr)   __INSTR_MODE instr
#define  _ADDR_MODE(instr)    __ADDR_MODE instr
#define  _DATA_MODE(instr)    __DATA_MODE instr
#define  _ADDR_SIZE(instr)    __ADDR_BITS instr
#define  _DUMMY_CYCLES(instr) __DUMMY_CYCLES instr

#define _CONFIG_INSTR(command, instr) do { \
    command.Instruction     = _CMD(instr); \
    command.InstructionMode = _INSTR_MODE(instr); \
    command.AddressMode     = _ADDR_MODE(instr); \
    command.DataMode        = _DATA_MODE(instr); \
    command.AddressSize     = _ADDR_SIZE(instr); \
    command.DummyCycles     = _DUMMY_CYCLES(instr); \
    command.AlternateByteMode  = QSPI_ALTERNATE_BYTES_NONE; \
    command.AlternateBytesSize = 0; \
    command.AlternateBytes     = 0; \
    command.DdrMode            = QSPI_DDR_MODE_DISABLE; \
    command.DdrHoldHalfCycle   = QSPI_DDR_HHC_ANALOG_DELAY; \
    command.SIOOMode           = QSPI_SIOO_INST_EVERY_CMD; \
    }while(0)

// W25Q64JV winbond
#define FLASH_ID             0x4016
#define FLASH_MF_ID          0xEF

// format: (cmd, instrLine, addrLine, dataLine, addrBits, dummyCycles)
#define INSTR_RESET_ENABLE (0x66, 1, 0, 0, 0, 0)
#define INSTR_RESET_DEVICE (0x99, 1, 0, 0, 0, 0)
#define INSTR_READ_JEDEC_ID     (0x9f, 1, 0, 1, 0, 0)
#define INSTR_WRITE_ENABLE (0x06, 1, 0, 0, 0, 0)
#define INSTR_READ_STATUS_REG1  (0x05, 1, 0, 1, 0, 0)
#define INSTR_WRITE_STATUS_REG1 (0x01, 1, 0, 1, 0, 0)
#define INSTR_READ_STATUS_REG2  (0x35, 1, 0, 1, 0, 0)
#define INSTR_WRITE_STATUS_REG2 (0x31, 1, 0, 1, 0, 0)
#define INSTR_READ_STATUS_REG3  (0x15, 1, 0, 1, 0, 0)
#define INSTR_WRITE_STATUS_REG3 (0x11, 1, 0, 1, 0, 0)
#define INSTR_READ_DATA    (0x03, 1, 1, 1, 24, 0)
#define INSTR_PAGE_PROG    (0x02, 1, 1, 1, 24, 0)
#define INSTR_SECTOR_ERASE (0x20, 1, 1, 1, 24, 0)
#define INSTR_BLOCK_32KB_ERASE (0x52, 1, 1, 1, 24, 0)
#define INSTR_BLOCK_64KB_ERASE (0xd8, 1, 1, 1, 24, 0)
#define INSTR_CHIP_ERASE (0xc7, 1, 0, 0, 0, 0)

#define INSTR_DUAL_FAST_READ (0x3b, 1, 1, 2, 24, 8)
#define INSTR_QUAD_PAGE_PROG (0x32, 1, 1, 4, 24, 0)
#define INSTR_QUAD_FAST_READ (0x6b, 1, 1, 4, 24, 8)


/* Reset Operations */
#define CMD_RESET_ENABLE             0x66

// Reset Device
#define CMD_RESET_DEVICE             0x99

// Fast Read Dual Output
#define CMD_DUAL_OUT_FAST_READ       0x3B

// JEDEC ID
#define CMD_JEDEC_ID                 0x9F

// Write Enable
#define CMD_WRITE_ENABLE             0x06

// Read Status Register-1
#define CMD_READ_STATUS_REG1         0x05

// Page Program
#define CMD_PAGE_PROG                0x02

// Sector Erase (4KB)
#define CMD_SECTOR_ERASE             0x20

// Block Erase (32KB)
#define CMD_BLOCK_32KB_ERASE         0x52

// Block Erase (64KB)
#define CMD_BLOCK_64KB_ERASE         0xD8

// Chip Erase
#define CMD_CHIP_ERASE               0xC7 // 0x60

/* Identification Operations */
//#define READ_ID_CMD                          0x9E
//#define READ_SERIAL_FLASH_DISCO_PARAM_CMD    0x5A

/* Read Operations */
//#define READ_CMD                             0x03
//#define FAST_READ_CMD                        0x0B
//#define BURST_READ_WAP_CMD                   0x0C

//#define DUAL_OUT_FAST_READ_DTR_CMD           0x3D

//#define DUAL_INOUT_FAST_READ_CMD             0xBB

//#define QUAD_OUT_FAST_READ_CMD               0x6B

//#define QUAD_INOUT_FAST_READ_CMD             0xEB


/* Write Operations */
//#define WRITE_DISABLE_CMD                    0x04

/* Register Operations */
//#define WRITE_STATUS_REG1_CMD                0x01
//#define READ_STATUS_REG2_CMD                 0x35
//#define WRITE_STATUS_REG2_CMD                0x31
//#define READ_STATUS_REG3_CMD                 0x15
//#define WRITE_STATUS_REG3_CMD                0x11

//#define WRITE_ENABLE_STATUS_REG_CMD          0x50

/* Program Operations */

//#define QUAD_IN_FAST_PROG_CMD                0x32

/* Erase Operations */



//#define PROG_ERASE_RESUME_CMD                0x7A
//#define PROG_ERASE_SUSPEND_CMD               0x75

/* One-Time Programmable Operations */
//#define READ_UIC_ID_CMD                      0x4B
//#define PROG_SECURITY_REG_CMD                0x42

/* Default dummy clocks cycles */
#define DUMMY_CLOCK_CYCLES_READ                8
//#define DUMMY_CLOCK_CYCLES_READ_QUAD         10
//#define DUMMY_CLOCK_CYCLES_READ_DTR          6
//#define DUMMY_CLOCK_CYCLES_READ_QUAD_DTR     8



/* Flag Status Register */
#define STATUS_REG1_BUSY                   ((uint8_t)0x01)    /*!< busy */
#define STATUS_REG1_WEL                    ((uint8_t)0x02)    /*!< write enable */
#define STATUS_REG2_QE                     ((uint8_t)0x02)    /*!< quad enable */

/* altternate bytes */
#define W25Q_ALTERNATE_BYTE_M            0xFF

#define _ERROOR_NOTIFY() HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_R_Pin, GPIO_PIN_RESET)


extern QSPI_HandleTypeDef hqspi;
extern struct StorageInfo const StorageInfo;

static struct StorageInfo const *_info;
static bool _quadEnabled;

static int _ReadStatueReg(int index, uint8_t *reg);

void W25Q_Init(void)
{
    uint8_t statusReg2;

    _info = &StorageInfo;

    HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_R_Pin, GPIO_PIN_SET);
    if(_ReadStatueReg(1, &statusReg2) == 0)
    {
        _quadEnabled = (statusReg2 & STATUS_REG2_QE) ? true : false;
    }
    else
    {
        _quadEnabled = false;
    }
}

int W25Q_ReadId(JEDEC_ID_t *id)
{
    QSPI_CommandTypeDef cmd = {0};
    uint8_t buf[3];

    /* Initialize the read command */
//    cmd.InstructionMode = QSPI_INSTRUCTION_1_LINE;
//    cmd.Instruction = CMD_JEDEC_ID;
//    cmd.AddressMode = QSPI_ADDRESS_NONE;
//    cmd.DataMode = QSPI_DATA_1_LINE;
//    cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
//    cmd.AlternateBytesSize = 0;
//    cmd.AlternateBytes = 0;
//    cmd.DummyCycles = 0;
//    cmd.NbData = 3;
//    cmd.DdrMode = QSPI_DDR_MODE_DISABLE;
//    cmd.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
//    cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
    _CONFIG_INSTR(cmd, INSTR_READ_JEDEC_ID);
    cmd.NbData = sizeof(buf);
    if (HAL_QSPI_Command(&hqspi, &cmd, HAL_QPSI_TIMEOUT_DEFAULT_VALUE) != HAL_OK)
    {
        _ERROOR_NOTIFY();
        return -1;
    }

    /* Reception of the data */
    if (HAL_QSPI_Receive(&hqspi, buf, HAL_QPSI_TIMEOUT_DEFAULT_VALUE) != HAL_OK)
    {
        _ERROOR_NOTIFY();
        return -2;
    }

    id->manufacturer = buf[0];
    id->type = buf[1];
    id->capacity = buf[2];

    return 0;
}

static int _ReadStatueReg(int index, uint8_t *reg)
{
    QSPI_CommandTypeDef cmd = {0};

    switch(index)
    {
    case 0:
        _CONFIG_INSTR(cmd, INSTR_READ_STATUS_REG1);
        break;

    case 1:
        _CONFIG_INSTR(cmd, INSTR_READ_STATUS_REG2);
        break;

    case 2:
        _CONFIG_INSTR(cmd, INSTR_READ_STATUS_REG3);
        break;

    default:
        _ERROOR_NOTIFY();
        return -1;
    }

    cmd.NbData = 1;
    if (HAL_QSPI_Command(&hqspi, &cmd, HAL_QPSI_TIMEOUT_DEFAULT_VALUE) != HAL_OK)
    {
        _ERROOR_NOTIFY();
        return -2;
    }

    /* Reception of the data */
    if (HAL_QSPI_Receive(&hqspi, reg, HAL_QPSI_TIMEOUT_DEFAULT_VALUE) != HAL_OK)
    {
        _ERROOR_NOTIFY();
        return -3;
    }

    return 0;
}

static int _WriteStatusReg(int index, uint8_t *reg)
{
    QSPI_CommandTypeDef cmd = {0};

    switch(index)
    {
    case 0:
        _CONFIG_INSTR(cmd, INSTR_WRITE_STATUS_REG1);
        break;

    case 1:
        _CONFIG_INSTR(cmd, INSTR_WRITE_STATUS_REG2);
        break;

    case 2:
        _CONFIG_INSTR(cmd, INSTR_WRITE_STATUS_REG3);
        break;

    default:
        _ERROOR_NOTIFY();
        return -1;
    }

    cmd.NbData = 1;
    if (HAL_QSPI_Command(&hqspi, &cmd, HAL_QPSI_TIMEOUT_DEFAULT_VALUE) != HAL_OK)
    {
        _ERROOR_NOTIFY();
        return -2;
    }

    /* Transmission of the data */
    if (HAL_QSPI_Transmit(&hqspi, reg, HAL_QPSI_TIMEOUT_DEFAULT_VALUE) != HAL_OK)
    {
        _ERROOR_NOTIFY();
        return -1;
    }

    return 0;
}

int W25Q_WriteEnable(void)
{
    QSPI_CommandTypeDef cmd = {0};
    QSPI_AutoPollingTypeDef config = {0};

    /* Enable write operations ------------------------------------------ */
//    cmd.InstructionMode = QSPI_INSTRUCTION_1_LINE;
//    cmd.Instruction = CMD_WRITE_ENABLE;
//    cmd.AddressMode = QSPI_ADDRESS_NONE;
//    cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
//    cmd.DataMode = QSPI_DATA_NONE;
//    cmd.DummyCycles = 0;
//    cmd.DdrMode = QSPI_DDR_MODE_DISABLE;
//    cmd.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
//    cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
    _CONFIG_INSTR(cmd, INSTR_WRITE_ENABLE);
    if (HAL_QSPI_Command(&hqspi, &cmd, HAL_QSPI_TIMEOUT_DEFAULT_VALUE) != HAL_OK)
    {
        _ERROOR_NOTIFY();
        return -1;
    }

    /* Configure automatic polling mode to wait for write enabling ---- */
    config.Match = STATUS_REG1_WEL;
    config.Mask = STATUS_REG1_WEL;
    config.MatchMode = QSPI_MATCH_MODE_AND;
    config.StatusBytesSize = 1;
    config.Interval = 0x10;
    config.AutomaticStop = QSPI_AUTOMATIC_STOP_ENABLE;

    cmd.Instruction = _CMD(INSTR_READ_STATUS_REG1);
    cmd.AddressMode = _ADDR_MODE(INSTR_READ_STATUS_REG1);
    cmd.DataMode    = _DATA_MODE(INSTR_READ_STATUS_REG1);
    cmd.DummyCycles = _DUMMY_CYCLES(INSTR_READ_STATUS_REG1);
    if (HAL_QSPI_AutoPolling(&hqspi, &cmd, &config, HAL_QSPI_TIMEOUT_DEFAULT_VALUE) != HAL_OK)
    {
        _ERROOR_NOTIFY();
        return -2;
    }

    return 0;
}

int W25Q_EnableQuad(void)
{
    uint8_t statusReg2;

    if(_ReadStatueReg(1, &statusReg2) != 0)
    {
        _ERROOR_NOTIFY();
        return -1;
    }

    if(statusReg2 & STATUS_REG2_QE)
    {
        return 0;
    }

    statusReg2 |= STATUS_REG2_QE;

    return _WriteStatusReg(1, &statusReg2);
}

int W25Q_DisableQuad(void)
{
    uint8_t statusReg2;

    if(_ReadStatueReg(1, &statusReg2) != 0)
    {
        _ERROOR_NOTIFY();
        return -1;
    }

    if((statusReg2 & STATUS_REG2_QE) == 0)
    {
        return 0;
    }

    statusReg2 &= ~STATUS_REG2_QE;

    return _WriteStatusReg(1, &statusReg2);
}

static int _AutoPollingMemReady(void)
{
    QSPI_CommandTypeDef cmd = {0};
    QSPI_AutoPollingTypeDef config = {0};

    /* Configure automatic polling mode to wait for memory ready ------ */
//    cmd.InstructionMode = QSPI_INSTRUCTION_1_LINE;
//    cmd.Instruction = CMD_READ_STATUS_REG1;
//    cmd.AddressMode = QSPI_ADDRESS_NONE;
//    cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
//    cmd.DataMode = QSPI_DATA_1_LINE;
//    cmd.DummyCycles = 0;
//    cmd.DdrMode = QSPI_DDR_MODE_DISABLE;
//    cmd.DdrHoldHalfCycle  = QSPI_DDR_HHC_ANALOG_DELAY;
//    cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
    _CONFIG_INSTR(cmd, INSTR_READ_STATUS_REG1);

    config.Match = 0x00;
    config.Mask = STATUS_REG1_BUSY;
    config.MatchMode = QSPI_MATCH_MODE_AND;
    config.StatusBytesSize = 1;
    config.Interval = 0x10;
    config.AutomaticStop = QSPI_AUTOMATIC_STOP_ENABLE;

    if (HAL_QSPI_AutoPolling(&hqspi, &cmd, &config, HAL_QSPI_TIMEOUT_DEFAULT_VALUE) != HAL_OK)
    {
        _ERROOR_NOTIFY();
        return -1;
    }

    return 0;
}

static int _ResetMemory(void)
{
    QSPI_CommandTypeDef cmd = {0};

    /* Initialize the reset enable command */
    cmd.Instruction = _CMD(INSTR_RESET_ENABLE);
    cmd.InstructionMode = _INSTR_MODE(INSTR_RESET_ENABLE);
    cmd.AddressMode = QSPI_ADDRESS_NONE;
    cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
    cmd.DataMode = _DATA_MODE(INSTR_RESET_ENABLE);
    cmd.DummyCycles = 0;
    cmd.DdrMode = QSPI_DDR_MODE_DISABLE;
    cmd.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
    cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
    if (HAL_QSPI_Command(&hqspi, &cmd, HAL_QPSI_TIMEOUT_DEFAULT_VALUE) != HAL_OK)
    {
        _ERROOR_NOTIFY();
        return -1;
    }

    /* Send the reset memory command */
    cmd.Instruction = _CMD(INSTR_RESET_DEVICE);
    if (HAL_QSPI_Command(&hqspi, &cmd, HAL_QPSI_TIMEOUT_DEFAULT_VALUE) != HAL_OK)
    {
        _ERROOR_NOTIFY();
        return -2;
    }

    /* Configure automatic polling mode to wait the memory is ready */
    if (_AutoPollingMemReady() != 0)
    {
        _ERROOR_NOTIFY();
        return -3;
    }

    return 0;
}

int W25Q_Reset(void)
{
    // deinit HAL
    if (HAL_QSPI_DeInit(&hqspi) !=  HAL_OK)
    {
        _ERROOR_NOTIFY();
        return -1;
    }

    // init HAL
    if (HAL_QSPI_Init(&hqspi) != HAL_OK)
    {
        _ERROOR_NOTIFY();
        return -1;
    }

    /* QSPI memory reset */
    if (_ResetMemory() != 0) {
        return -1;
    }

    return 0;
}

int W25Q_FlashInfo(uint32_t *devStartAddr, uint32_t *eraseNum, uint32_t *eraseSize)
{

    if(devStartAddr != NULL) {
        *devStartAddr = (uint32_t) _info->DeviceStartAddress;
    }

    if (eraseNum != NULL) {
        *eraseNum = (uint32_t) _info->sectors[0].SectorNum;
    }

    if (eraseSize != NULL) {
        *eraseSize = (uint32_t) _info->sectors[0].SectorSize;
    }

    return 0;
}

int W25Q_EraseChip(void)
{
    QSPI_CommandTypeDef cmd = {0};

    /* Initialize the erase command */
//    cmd.InstructionMode = QSPI_INSTRUCTION_1_LINE;
//    cmd.Instruction = CMD_CHIP_ERASE;
//    cmd.AddressMode = QSPI_ADDRESS_NONE;
//    cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
//    cmd.DataMode = QSPI_DATA_NONE;
//    cmd.DummyCycles = 0;
//    cmd.DdrMode = QSPI_DDR_MODE_DISABLE;
//    cmd.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
//    cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
    _CONFIG_INSTR(cmd, INSTR_CHIP_ERASE);
    if (HAL_QSPI_Command(&hqspi, &cmd, HAL_QPSI_TIMEOUT_DEFAULT_VALUE) != HAL_OK)
    {
        _ERROOR_NOTIFY();
        return -1;
    }

    /* Configure automatic polling mode to wait for end of erase */
    if (_AutoPollingMemReady() != 0)
    {
        _ERROOR_NOTIFY();
        return -2;
    }

    return 0;
}

int W25Q_ErasePart(uint32_t addr)
{
    QSPI_CommandTypeDef cmd = {0};

    switch(_info->sectors[0].SectorSize)
    {
    case 4 * 1024:
        _CONFIG_INSTR(cmd, INSTR_SECTOR_ERASE);
        break;

    case 32 * 1024:
        _CONFIG_INSTR(cmd, INSTR_BLOCK_32KB_ERASE);
        break;

    case 64 * 1024:
        _CONFIG_INSTR(cmd, INSTR_BLOCK_64KB_ERASE);
        break;

    default:
        return -1;
    }

    /* Initialize the erase command */
//    cmd.InstructionMode = QSPI_INSTRUCTION_1_LINE;
//    cmd.Instruction = eraseCmd;
//    cmd.AddressMode = QSPI_ADDRESS_1_LINE;
//    cmd.AddressSize = QSPI_ADDRESS_24_BITS;
//    cmd.Address = addr;
//    cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
//    cmd.DataMode = QSPI_DATA_NONE;
//    cmd.DummyCycles = 0;
//    cmd.DdrMode = QSPI_DDR_MODE_DISABLE;
//    cmd.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
//    cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
    cmd.Address = addr;
    if (HAL_QSPI_Command(&hqspi, &cmd, HAL_QPSI_TIMEOUT_DEFAULT_VALUE) != HAL_OK)
    {
        _ERROOR_NOTIFY();
        return -1;
    }

    /* Configure automatic polling mode to wait for end of erase */
    if (_AutoPollingMemReady() != 0)
    {
        return -2;
    }

    return 0;
}

int W25Q_Read(uint32_t addr, void *data, uint32_t len)
{
    QSPI_CommandTypeDef cmd = {0};

    /* Initialize the read command */
//    cmd.InstructionMode = QSPI_INSTRUCTION_1_LINE;
//    cmd.Instruction = CMD_DUAL_OUT_FAST_READ;
//    cmd.AddressMode = QSPI_ADDRESS_1_LINE;
//    cmd.AddressSize = QSPI_ADDRESS_24_BITS;
//    cmd.Address = addr;
//    cmd.DataMode = QSPI_DATA_2_LINES;
//    cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
//    cmd.AlternateBytesSize = 0;
//    cmd.AlternateBytes = 0;
//    cmd.DummyCycles = DUMMY_CLOCK_CYCLES_READ;
//    cmd.NbData = len;
//    cmd.DdrMode = QSPI_DDR_MODE_DISABLE;
//    cmd.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
//    cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
#if 0
    if(_quadEnabled)
    {
        _CONFIG_INSTR(cmd, INSTR_QUAD_FAST_READ);
    }
    else
#endif
    {
        _CONFIG_INSTR(cmd, INSTR_READ_DATA);
    }

    cmd.Address = addr;
    cmd.NbData = len;
    if(HAL_QSPI_Command(&hqspi, &cmd, HAL_QPSI_TIMEOUT_DEFAULT_VALUE) != HAL_OK)
    {
        _ERROOR_NOTIFY();
        return -1;
    }

    /* Reception of the data */
    if(HAL_QSPI_Receive(&hqspi, data, HAL_QPSI_TIMEOUT_DEFAULT_VALUE) != HAL_OK)
    {
        _ERROOR_NOTIFY();
        return -2;
    }

    return 0;
}

int W25Q_Write(uint32_t addr, const void *data, uint32_t len)
{
    const uint8_t *dat = data;
    QSPI_CommandTypeDef cmd = {0};
    uint32_t pageSize = (uint32_t) _info->PageSize;
    uint32_t end_addr, current_size, current_addr;

    if (len == 0)
        return 0;

    /* Calculation of the size between the write address and the end of the page */
    current_size = pageSize - (addr % pageSize);

    /* Check if the size of the data is less than the remaining place in the page */
    if (current_size > len) {
        current_size = len;
    }

    /* Initialize the adress variables */
    current_addr = addr;
    end_addr = addr + len;

    /* Initialize the program command */
//    cmd.InstructionMode = QSPI_INSTRUCTION_1_LINE;
//    cmd.Instruction = CMD_PAGE_PROG;
//    cmd.AddressMode = QSPI_ADDRESS_1_LINE;
//    cmd.DataMode = QSPI_DATA_1_LINE;
//    cmd.AddressSize = QSPI_ADDRESS_24_BITS;
//    cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
//    cmd.DummyCycles = 0;
//    cmd.DdrMode = QSPI_DDR_MODE_DISABLE;
//    cmd.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
//    cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
    _CONFIG_INSTR(cmd, INSTR_PAGE_PROG);

    /* Perform the write page by page */
    do {
        cmd.Address = current_addr;
        cmd.NbData = current_size;

        /* Configure the command */
        if (HAL_QSPI_Command(&hqspi, &cmd, HAL_QPSI_TIMEOUT_DEFAULT_VALUE) != HAL_OK)
        {
            _ERROOR_NOTIFY();
            return -1;
        }

        /* Transmission of the data */
        if (HAL_QSPI_Transmit(&hqspi, (uint8_t *)dat, HAL_QPSI_TIMEOUT_DEFAULT_VALUE) != HAL_OK)
        {
            _ERROOR_NOTIFY();
            return -1;
        }

        /* Configure automatic polling mode to wait for end of program */
        if (_AutoPollingMemReady() != 0)
        {
            return -1;
        }

        /* Update the address and size variables for next page programming */
        current_addr += current_size;
        dat += current_size;
        current_size = ((current_addr + pageSize) > end_addr) ? (end_addr - current_addr) : pageSize;
    } while (current_addr < end_addr);

    return 0;
}

int W25Q_MemoryMapped(void)
{
    QSPI_CommandTypeDef cmd = {0};
    QSPI_MemoryMappedTypeDef config = {0};

    /* Configure the command for the read instruction */
    cmd.InstructionMode = QSPI_INSTRUCTION_1_LINE;
    cmd.Instruction = CMD_DUAL_OUT_FAST_READ;
    cmd.AddressMode = QSPI_ADDRESS_1_LINE;
    cmd.AddressSize = QSPI_ADDRESS_24_BITS;
    cmd.DataMode = QSPI_DATA_2_LINES;
    cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
    cmd.AlternateBytesSize = 0;
    cmd.AlternateBytes = 0;
    cmd.DummyCycles = DUMMY_CLOCK_CYCLES_READ;
    cmd.DdrMode = QSPI_DDR_MODE_DISABLE;
    cmd.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
    cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;

    /* Configure the memory mapped mode */
    config.TimeOutActivation = QSPI_TIMEOUT_COUNTER_DISABLE;
    config.TimeOutPeriod = 0;

    if (HAL_QSPI_MemoryMapped(&hqspi, &cmd, &config) != HAL_OK)
    {
        _ERROOR_NOTIFY();
        return -1;;
    }

    return 0;
}
